/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; Copyright (c) 2020 STMicroelectronics.
  * All rights reserved.</center></h2>
  *
  * This software component is licensed by ST under BSD 3-Clause license,
  * the "License"; You may not use this file except in compliance with the
  * License. You may obtain a copy of the License at:
  *                        opensource.org/licenses/BSD-3-Clause
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "stepping_motor_conf.h"
#include <math.h>
#include <string.h>

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
#define TEST

/* 串口1的接收字符串大小 */
#define UART1_STRING_SIZE 30

/*openmv*/
#define OPENMV_FRAME_SIZE_X 230
#define OPENMV_FRAME_SIZE_Y 230
#define OPENMV_NX 0    /*openmv反向x*/
#define OPENMV_NY 1    /*openmv反向y*/
#define MOTOR_B_ND 0   /*主轴电机反向*/

/*fix*/
#define FIX_X (-2)
#define FIX_Y 182

/*z stage*/
#define Z_STAGE_0 (-110)
#define Z_STAGE_1 60
#define Z_STAGE_2 100

/*motor d*/
#define MOTOR_D_V_LOW  25
#define MOTOR_D_V_HIGH 33

#define delay HAL_Delay
#define DELAY_DEFAULT 1000


/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
ADC_HandleTypeDef hadc1;

DAC_HandleTypeDef hdac;

TIM_HandleTypeDef htim3;
TIM_HandleTypeDef htim4;

UART_HandleTypeDef huart1;
UART_HandleTypeDef huart2;
UART_HandleTypeDef huart3;

/* USER CODE BEGIN PV */
struct
{
  uint32_t x_counter;
  uint32_t y_counter;
  uint32_t z_counter;
  int32_t  x;
  int32_t  y;
  int32_t  z;
} stepping_motor;

/* 串口1用于接收电脑发送的命令字符串 */
struct
{
  uint8_t string[UART1_STRING_SIZE];
  int8_t  string_index;   /* 字符串索引，值为-1时接收完毕 */
  uint8_t buffer;
} uart1;

struct
{
    uint8_t string[3];
    int8_t  string_index;
    uint8_t buffer;
    uint8_t x;
    uint8_t y;
} uart3;

struct
{
    int32_t x1;
    int32_t y1;
    uint8_t auto1;
    int32_t x2;
    int32_t y2;
    uint8_t auto2;
    int32_t x3;
    int32_t y3;
    uint8_t auto3;
    int32_t x4;
    int32_t y4;

    int32_t fix1_x;
    int32_t fix1_y;
    int32_t fix2_x;
    int32_t fix2_y;
    int32_t fix3_x;
    int32_t fix3_y;
} plan;
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_ADC1_Init(void);
static void MX_DAC_Init(void);
static void MX_TIM3_Init(void);
static void MX_TIM4_Init(void);
static void MX_USART1_UART_Init(void);
static void MX_USART2_UART_Init(void);
static void MX_USART3_UART_Init(void);
/* USER CODE BEGIN PFP */
void move_xy_step(int32_t x, int32_t y);
void move_xy_01mm(int32_t x, int32_t y);
void move_z_step(int32_t z);
void move_z_01mm(int32_t z);
void moveto_xy_step(int32_t x, int32_t y);
void moveto_xy_01mm(int32_t x, int32_t y);
void moveto_z_step(int32_t z);
void moveto_z_01mm(int32_t z);
void motor_x_move(uint8_t dir);
void motor_x_stop(uint8_t dir);
void motor_y_move(uint8_t dir);
void motor_y_stop(uint8_t dir);
void init(void);
void refresh_uart1(void); /*处理串口1*/
void autolocate(void);
void set0xy(void); /*set x, y to 0*/
void set0z(void);  /*set z to 0*/
void set0(void);   /*set x, y, z to 0*/
uint8_t str2int(const char *string, int32_t *number);
uint8_t int2str(const int32_t* number, char* string);
void set_DAC(uint8_t v);
void set_BLDC_D(uint8_t dir);
void drill(void);
void run(void);
void run_test(void);

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_ADC1_Init();
  MX_DAC_Init();
  MX_TIM3_Init();
  MX_TIM4_Init();
  MX_USART1_UART_Init();
  MX_USART2_UART_Init();
  MX_USART3_UART_Init();
  /* USER CODE BEGIN 2 */

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  init();
  while (1)
  {
      refresh_uart1();
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Configure the main internal regulator output voltage
  */
  __HAL_RCC_PWR_CLK_ENABLE();
  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLM = 4;
  RCC_OscInitStruct.PLL.PLLN = 168;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
  RCC_OscInitStruct.PLL.PLLQ = 4;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }
  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK)
  {
    Error_Handler();
  }
}

/**
  * @brief ADC1 Initialization Function
  * @param None
  * @retval None
  */
static void MX_ADC1_Init(void)
{

  /* USER CODE BEGIN ADC1_Init 0 */

  /* USER CODE END ADC1_Init 0 */

  ADC_ChannelConfTypeDef sConfig = {0};

  /* USER CODE BEGIN ADC1_Init 1 */

  /* USER CODE END ADC1_Init 1 */
  /** Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)
  */
  hadc1.Instance = ADC1;
  hadc1.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4;
  hadc1.Init.Resolution = ADC_RESOLUTION_12B;
  hadc1.Init.ScanConvMode = DISABLE;
  hadc1.Init.ContinuousConvMode = DISABLE;
  hadc1.Init.DiscontinuousConvMode = DISABLE;
  hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
  hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START;
  hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
  hadc1.Init.NbrOfConversion = 1;
  hadc1.Init.DMAContinuousRequests = DISABLE;
  hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
  if (HAL_ADC_Init(&hadc1) != HAL_OK)
  {
    Error_Handler();
  }
  /** Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time.
  */
  sConfig.Channel = ADC_CHANNEL_1;
  sConfig.Rank = 1;
  sConfig.SamplingTime = ADC_SAMPLETIME_3CYCLES;
  if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN ADC1_Init 2 */

  /* USER CODE END ADC1_Init 2 */

}

/**
  * @brief DAC Initialization Function
  * @param None
  * @retval None
  */
static void MX_DAC_Init(void)
{

  /* USER CODE BEGIN DAC_Init 0 */

  /* USER CODE END DAC_Init 0 */

  DAC_ChannelConfTypeDef sConfig = {0};

  /* USER CODE BEGIN DAC_Init 1 */

  /* USER CODE END DAC_Init 1 */
  /** DAC Initialization
  */
  hdac.Instance = DAC;
  if (HAL_DAC_Init(&hdac) != HAL_OK)
  {
    Error_Handler();
  }
  /** DAC channel OUT1 config
  */
  sConfig.DAC_Trigger = DAC_TRIGGER_NONE;
  sConfig.DAC_OutputBuffer = DAC_OUTPUTBUFFER_ENABLE;
  if (HAL_DAC_ConfigChannel(&hdac, &sConfig, DAC_CHANNEL_1) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN DAC_Init 2 */

  /* USER CODE END DAC_Init 2 */

}

/**
  * @brief TIM3 Initialization Function
  * @param None
  * @retval None
  */
static void MX_TIM3_Init(void)
{

  /* USER CODE BEGIN TIM3_Init 0 */

  /* USER CODE END TIM3_Init 0 */

  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
  TIM_MasterConfigTypeDef sMasterConfig = {0};
  TIM_OC_InitTypeDef sConfigOC = {0};

  /* USER CODE BEGIN TIM3_Init 1 */

  /* USER CODE END TIM3_Init 1 */
  htim3.Instance = TIM3;
  htim3.Init.Prescaler = 65535;
  htim3.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim3.Init.Period = 1;
  htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
  if (HAL_TIM_Base_Init(&htim3) != HAL_OK)
  {
    Error_Handler();
  }
  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
  if (HAL_TIM_ConfigClockSource(&htim3, &sClockSourceConfig) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_TIM_PWM_Init(&htim3) != HAL_OK)
  {
    Error_Handler();
  }
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK)
  {
    Error_Handler();
  }
  sConfigOC.OCMode = TIM_OCMODE_PWM1;
  sConfigOC.Pulse = 1;
  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
  if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN TIM3_Init 2 */

  /* USER CODE END TIM3_Init 2 */
  HAL_TIM_MspPostInit(&htim3);

}

/**
  * @brief TIM4 Initialization Function
  * @param None
  * @retval None
  */
static void MX_TIM4_Init(void)
{

  /* USER CODE BEGIN TIM4_Init 0 */

  /* USER CODE END TIM4_Init 0 */

  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
  TIM_MasterConfigTypeDef sMasterConfig = {0};
  TIM_OC_InitTypeDef sConfigOC = {0};

  /* USER CODE BEGIN TIM4_Init 1 */

  /* USER CODE END TIM4_Init 1 */
  htim4.Instance = TIM4;
  htim4.Init.Prescaler = 65535;
  htim4.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim4.Init.Period = 1;
  htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
  if (HAL_TIM_Base_Init(&htim4) != HAL_OK)
  {
    Error_Handler();
  }
  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
  if (HAL_TIM_ConfigClockSource(&htim4, &sClockSourceConfig) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_TIM_PWM_Init(&htim4) != HAL_OK)
  {
    Error_Handler();
  }
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK)
  {
    Error_Handler();
  }
  sConfigOC.OCMode = TIM_OCMODE_PWM1;
  sConfigOC.Pulse = 1;
  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
  if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN TIM4_Init 2 */

  /* USER CODE END TIM4_Init 2 */
  HAL_TIM_MspPostInit(&htim4);

}

/**
  * @brief USART1 Initialization Function
  * @param None
  * @retval None
  */
static void MX_USART1_UART_Init(void)
{

  /* USER CODE BEGIN USART1_Init 0 */

  /* USER CODE END USART1_Init 0 */

  /* USER CODE BEGIN USART1_Init 1 */

  /* USER CODE END USART1_Init 1 */
  huart1.Instance = USART1;
  huart1.Init.BaudRate = 115200;
  huart1.Init.WordLength = UART_WORDLENGTH_8B;
  huart1.Init.StopBits = UART_STOPBITS_1;
  huart1.Init.Parity = UART_PARITY_NONE;
  huart1.Init.Mode = UART_MODE_TX_RX;
  huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart1.Init.OverSampling = UART_OVERSAMPLING_16;
  if (HAL_UART_Init(&huart1) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN USART1_Init 2 */

  /* USER CODE END USART1_Init 2 */

}

/**
  * @brief USART2 Initialization Function
  * @param None
  * @retval None
  */
static void MX_USART2_UART_Init(void)
{

  /* USER CODE BEGIN USART2_Init 0 */

  /* USER CODE END USART2_Init 0 */

  /* USER CODE BEGIN USART2_Init 1 */

  /* USER CODE END USART2_Init 1 */
  huart2.Instance = USART2;
  huart2.Init.BaudRate = 115200;
  huart2.Init.WordLength = UART_WORDLENGTH_8B;
  huart2.Init.StopBits = UART_STOPBITS_1;
  huart2.Init.Parity = UART_PARITY_NONE;
  huart2.Init.Mode = UART_MODE_TX_RX;
  huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart2.Init.OverSampling = UART_OVERSAMPLING_16;
  if (HAL_UART_Init(&huart2) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN USART2_Init 2 */

  /* USER CODE END USART2_Init 2 */

}

/**
  * @brief USART3 Initialization Function
  * @param None
  * @retval None
  */
static void MX_USART3_UART_Init(void)
{

  /* USER CODE BEGIN USART3_Init 0 */

  /* USER CODE END USART3_Init 0 */

  /* USER CODE BEGIN USART3_Init 1 */

  /* USER CODE END USART3_Init 1 */
  huart3.Instance = USART3;
  huart3.Init.BaudRate = 115200;
  huart3.Init.WordLength = UART_WORDLENGTH_8B;
  huart3.Init.StopBits = UART_STOPBITS_1;
  huart3.Init.Parity = UART_PARITY_NONE;
  huart3.Init.Mode = UART_MODE_TX_RX;
  huart3.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart3.Init.OverSampling = UART_OVERSAMPLING_16;
  if (HAL_UART_Init(&huart3) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN USART3_Init 2 */

  /* USER CODE END USART3_Init 2 */

}

/**
  * @brief GPIO Initialization Function
  * @param None
  * @retval None
  */
static void MX_GPIO_Init(void)
{
  GPIO_InitTypeDef GPIO_InitStruct = {0};

  /* GPIO Ports Clock Enable */
  __HAL_RCC_GPIOH_CLK_ENABLE();
  __HAL_RCC_GPIOA_CLK_ENABLE();
  __HAL_RCC_GPIOB_CLK_ENABLE();
  __HAL_RCC_GPIOD_CLK_ENABLE();
  __HAL_RCC_GPIOG_CLK_ENABLE();

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(GPIOD, MOTOR_X_DIR_Pin|MOTOR_X_ENA_Pin|MOTOR_Y_DIR_Pin|MOTOR_Y_ENA_Pin
                          |MOTOR_Z_DIR_Pin|MOTOR_Z_ENA_Pin, GPIO_PIN_SET);

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(GPIOD, GPIO_PIN_6, GPIO_PIN_RESET);

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(MOTOR_B_DIR_GPIO_Port, MOTOR_B_DIR_Pin, GPIO_PIN_SET);

  /*Configure GPIO pins : MOTOR_X_DIR_Pin MOTOR_X_ENA_Pin MOTOR_Y_DIR_Pin MOTOR_Y_ENA_Pin
                           MOTOR_Z_DIR_Pin MOTOR_Z_ENA_Pin PD6 */
  GPIO_InitStruct.Pin = MOTOR_X_DIR_Pin|MOTOR_X_ENA_Pin|MOTOR_Y_DIR_Pin|MOTOR_Y_ENA_Pin
                          |MOTOR_Z_DIR_Pin|MOTOR_Z_ENA_Pin|GPIO_PIN_6;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);

  /*Configure GPIO pin : PD7 */
  GPIO_InitStruct.Pin = GPIO_PIN_7;
  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);

  /*Configure GPIO pin : MOTOR_B_BRK_Pin */
  GPIO_InitStruct.Pin = MOTOR_B_DIR_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(MOTOR_B_DIR_GPIO_Port, &GPIO_InitStruct);
}

/* USER CODE BEGIN 4 */

void init(void)

{
    /*Stepping Motor Init*/
    if(FREE_MOTOR_WHILE_STOP)
    {
        HAL_GPIO_WritePin(MOTOR_X_ENA_GPIO_Port, MOTOR_X_ENA_Pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(MOTOR_Y_ENA_GPIO_Port, MOTOR_Y_ENA_Pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(MOTOR_Z_ENA_GPIO_Port, MOTOR_Z_ENA_Pin, GPIO_PIN_SET);
    }
    else
    {
     HAL_GPIO_WritePin(MOTOR_X_ENA_GPIO_Port, MOTOR_X_ENA_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(MOTOR_Y_ENA_GPIO_Port, MOTOR_Y_ENA_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(MOTOR_Z_ENA_GPIO_Port, MOTOR_Z_ENA_Pin, GPIO_PIN_RESET);
    }

    /*串口1 Init*/
    uart1.string[0]    = '\0';
    uart1.string_index = 0;
    uart1.buffer       = 0;
    HAL_UART_Receive_IT(&huart1, &uart1.buffer, 1);

    /*串口2 Init*/

    /*主轴电机*/
    set_DAC(0);
    set_BLDC_D(1);

    /*plan*/
    plan.x1 = 0;
    plan.y1 = 0;
    plan.x2 = 0;
    plan.y2 = 0;
    plan.x3 = 0;
    plan.y3 = 0;
    plan.x4 = 0;
    plan.y4 = 0;
    plan.auto1 = 0;
    plan.auto2 = 0;
    plan.auto3 = 0;
    plan.fix1_x = FIX_X;
    plan.fix1_y = FIX_Y;
    plan.fix2_x = FIX_X;
    plan.fix2_y = FIX_Y;
    plan.fix3_x = FIX_X;
    plan.fix3_y = FIX_Y;
}

/**
 * 水平移动，单位：step
*/
void move_xy_step(int32_t x, int32_t y)
{
    int8_t dir_x, dir_y;

    if(x != 0)
    {
        uint32_t temp = MIN_PUL_TIM_PSC_XY * sqrt(1 + pow((double) y, 2) / pow((double) x, 2));
        if(temp > 0xFFFF)
            temp = 0xFFFF;
        htim3.Instance->PSC = temp; /*x*/
    }
    if(y != 0)
    {
        uint32_t temp = MIN_PUL_TIM_PSC_XY * sqrt(1 + pow((double) x, 2) / pow((double) y, 2));
        if(temp > 0xFFFF)
            temp = 0xFFFF;
        htim4.Instance->PSC = temp; /*y*/
    }

    /*ENA*/
    if(FREE_MOTOR_WHILE_STOP)
    {
        HAL_GPIO_WritePin(MOTOR_X_ENA_GPIO_Port, MOTOR_X_ENA_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(MOTOR_Y_ENA_GPIO_Port, MOTOR_Y_ENA_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(MOTOR_Z_ENA_GPIO_Port, MOTOR_Z_ENA_Pin, GPIO_PIN_RESET);
        delay(1);
    }

    /*DIR*/
    if(x >= 0)
    {
        HAL_GPIO_WritePin(MOTOR_X_DIR_GPIO_Port, MOTOR_X_DIR_Pin, GPIO_PIN_SET);
        dir_x = 1;
    }
    else
    {
        HAL_GPIO_WritePin(MOTOR_X_DIR_GPIO_Port, MOTOR_X_DIR_Pin, GPIO_PIN_RESET);
        x = -x;
        dir_x = -1;
    }
    if(y >= 0)
    {
        HAL_GPIO_WritePin(MOTOR_Y_DIR_GPIO_Port, MOTOR_Y_DIR_Pin, GPIO_PIN_SET);
        dir_y = 1;
    }
    else
    {
        HAL_GPIO_WritePin(MOTOR_Y_DIR_GPIO_Port, MOTOR_Y_DIR_Pin, GPIO_PIN_RESET);
        y = -y;
        dir_y = -1;
    }
    delay(1);

    /*PUL*/
    stepping_motor.x_counter = 0;
    stepping_motor.y_counter = 0;
    if(x != 0)
        HAL_TIM_PWM_Start_IT(&htim3, TIM_CHANNEL_1);
    if(y != 0)
        HAL_TIM_PWM_Start_IT(&htim4, TIM_CHANNEL_1);
    while(stepping_motor.x_counter < x && stepping_motor.y_counter < y);
    if(stepping_motor.x_counter >= x)
    {
        HAL_TIM_PWM_Stop_IT(&htim3, TIM_CHANNEL_1);
        while(stepping_motor.y_counter < y);
        HAL_TIM_PWM_Stop_IT(&htim4, TIM_CHANNEL_1);
    }
    else
    {
        HAL_TIM_PWM_Stop_IT(&htim4, TIM_CHANNEL_1);
        while(stepping_motor.x_counter < x);
        HAL_TIM_PWM_Stop_IT(&htim3, TIM_CHANNEL_1);
    }
    delay(1);

    /*ENA*/
    if(FREE_MOTOR_WHILE_STOP)
    {
        HAL_GPIO_WritePin(MOTOR_X_ENA_GPIO_Port, MOTOR_X_ENA_Pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(MOTOR_Y_ENA_GPIO_Port, MOTOR_Y_ENA_Pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(MOTOR_Z_ENA_GPIO_Port, MOTOR_Z_ENA_Pin, GPIO_PIN_SET);
        delay(1);
    }

    stepping_motor.x += dir_x * stepping_motor.x_counter;
    stepping_motor.y += dir_y * stepping_motor.y_counter;
}

/**
 * 水平移动，单位：0.1mm
*/
void move_xy_01mm(int32_t x, int32_t y)
{
    move_xy_step(x * 249, y * 249);
}

/**
 * Z轴移动，单位：step
*/
void move_z_step(int32_t z)
{
    int8_t dir_z;

    htim3.Instance->PSC = MIN_PUL_TIM_PSC_Z;

    /*ENA*/
    if(FREE_MOTOR_WHILE_STOP)
    {
        HAL_GPIO_WritePin(MOTOR_Z_ENA_GPIO_Port, MOTOR_Z_ENA_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(MOTOR_Y_ENA_GPIO_Port, MOTOR_Y_ENA_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(MOTOR_Z_ENA_GPIO_Port, MOTOR_Z_ENA_Pin, GPIO_PIN_RESET);
        delay(1);
    }

    /*DIR*/
    if(z >= 0)
    {
        HAL_GPIO_WritePin(MOTOR_Z_DIR_GPIO_Port, MOTOR_Z_DIR_Pin, GPIO_PIN_SET);
        dir_z = 1;
    }
    else
    {
        HAL_GPIO_WritePin(MOTOR_Z_DIR_GPIO_Port, MOTOR_Z_DIR_Pin, GPIO_PIN_RESET);
        z = -z;
        dir_z = -1;
    }
    delay(1);

    /*PUL*/
    stepping_motor.z_counter = 0;
    if(z != 0)
        HAL_TIM_PWM_Start_IT(&htim3, TIM_CHANNEL_2);
    while(stepping_motor.z_counter < z);
    HAL_TIM_PWM_Stop_IT(&htim3, TIM_CHANNEL_2);
    delay(1);

    /*ENA*/
    if(FREE_MOTOR_WHILE_STOP)
    {
        HAL_GPIO_WritePin(MOTOR_Z_ENA_GPIO_Port, MOTOR_Z_ENA_Pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(MOTOR_Y_ENA_GPIO_Port, MOTOR_Y_ENA_Pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(MOTOR_Z_ENA_GPIO_Port, MOTOR_Z_ENA_Pin, GPIO_PIN_SET);
        delay(1);
    }

    stepping_motor.z += dir_z * stepping_motor.z_counter;
}

/**
 * Z轴移动，单位：0.1mm
*/
void move_z_01mm(int32_t z)
{
    move_z_step(z * 249);
}

void motor_x_move(uint8_t dir)
{
    if(FREE_MOTOR_WHILE_STOP)
    {
        HAL_GPIO_WritePin(MOTOR_X_ENA_GPIO_Port, MOTOR_X_ENA_Pin, GPIO_PIN_RESET);
        delay(1);
    }

    if(dir == 0)
    {
        HAL_GPIO_WritePin(MOTOR_X_DIR_GPIO_Port, MOTOR_X_DIR_Pin, GPIO_PIN_RESET);
    }
    else
    {
        HAL_GPIO_WritePin(MOTOR_X_DIR_GPIO_Port, MOTOR_X_DIR_Pin, GPIO_PIN_SET);
    }
    delay(1);

    htim3.Instance->PSC = SLOW_PUL_TIM_PSC_XY;
    stepping_motor.x_counter = 0;
    HAL_TIM_PWM_Start_IT(&htim3, TIM_CHANNEL_1);
}

void motor_x_stop(uint8_t dir)
{
    HAL_TIM_PWM_Stop_IT(&htim3, TIM_CHANNEL_1);
    delay(1);

    if(FREE_MOTOR_WHILE_STOP)
    {
        HAL_GPIO_WritePin(MOTOR_X_ENA_GPIO_Port, MOTOR_X_ENA_Pin, GPIO_PIN_SET);
        delay(1);
    }

    if(dir == 0)
    {
        stepping_motor.x -= stepping_motor.x_counter;
    }
    else
    {
        stepping_motor.x += stepping_motor.x_counter;
    }
}

void motor_y_move(uint8_t dir)
{
    if(FREE_MOTOR_WHILE_STOP)
    {
        HAL_GPIO_WritePin(MOTOR_Y_ENA_GPIO_Port, MOTOR_Y_ENA_Pin, GPIO_PIN_RESET);
        delay(1);
    }

    if(dir == 0)
    {
        HAL_GPIO_WritePin(MOTOR_Y_DIR_GPIO_Port, MOTOR_Y_DIR_Pin, GPIO_PIN_RESET);
    }
    else
    {
        HAL_GPIO_WritePin(MOTOR_Y_DIR_GPIO_Port, MOTOR_Y_DIR_Pin, GPIO_PIN_SET);
    }
    delay(1);

    htim4.Instance->PSC = SLOW_PUL_TIM_PSC_XY;
    stepping_motor.y_counter = 0;
    HAL_TIM_PWM_Start_IT(&htim4, TIM_CHANNEL_1);
}

void motor_y_stop(uint8_t dir)
{
    HAL_TIM_PWM_Stop_IT(&htim4, TIM_CHANNEL_1);
    delay(1);

    if(FREE_MOTOR_WHILE_STOP)
    {
        HAL_GPIO_WritePin(MOTOR_Y_ENA_GPIO_Port, MOTOR_Y_ENA_Pin, GPIO_PIN_SET);
        delay(1);
    }

    if(dir == 0)
    {
        stepping_motor.y -= stepping_motor.y_counter;
    }
    else
    {
        stepping_motor.y += stepping_motor.y_counter;
    }
}

void moveto_xy_step(int32_t x, int32_t y)
{
    move_xy_step(x - stepping_motor.x, y - stepping_motor.y);
}

void moveto_xy_01mm(int32_t x, int32_t y)
{
    moveto_xy_step(x * 249, y * 249);
}

void moveto_z_step(int32_t z)
{
    move_z_step(z - stepping_motor.z);
}

void moveto_z_01mm(int32_t z)
{
    moveto_z_step(z * 249);
}

void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim)
{
  if(htim == &htim3)
  {/* htim3 */
    stepping_motor.x_counter += 1;
    stepping_motor.z_counter += 1;
  }
  else if(htim == &htim4)
  {/* htim4 */
    stepping_motor.y_counter += 1;
  }
  else
  {
    
  }
}

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
  if(huart == &huart1)
  {/* huart1 */
    uart1.string[uart1.string_index] = uart1.buffer;
    uart1.string_index += 1;
    if(uart1.string_index >= UART1_STRING_SIZE)
      uart1.string_index = 0;
    if(uart1.buffer == '\n' || uart1.buffer == '\r')
    {
      uart1.string[uart1.string_index] = '\0';
      uart1.string_index = -1; /* 接收完毕标志 */
    }
    else
    {
      HAL_UART_Receive_IT(&huart1, &uart1.buffer, 1 * sizeof(uint8_t));
    }
  }
  else if(huart == &huart2)
  {/* huart2 */
//      uart2.string[uart2.string_index] = uart2.buffer;
//      uart2.string_index += 1;
//      if(uart2.string_index >= 3)
//          uart2.string_index = 0;
//      if(uart2.buffer == 0xFF)
//      {
//          if(uart2.string_index == 0)
//          {
//              uart2.x = uart2.string[0];
//              uart2.y = uart2.string[1];
//          }
//          else
//          {
//              uart2.string_index = 0;
//          }
//      }
//      HAL_UART_Receive_IT(&huart2, &uart2.buffer, 1 * sizeof(uint8_t));
  }
  else if(huart == &huart3)
  {/* huart3 */
      uart3.string[uart3.string_index] = uart3.buffer;
      uart3.string_index += 1;
      if(uart3.string_index >= 3)
          uart3.string_index = 0;
      if(uart3.buffer == 0xFF)
      {
          if(uart3.string_index == 0)
          {
              uart3.x = uart3.string[0];
              uart3.y = uart3.string[1];
          }
          else
          {
              uart3.string_index = 0;
          }
      }
      HAL_UART_Receive_IT(&huart3, &uart3.buffer, 1 * sizeof(uint8_t));
  }
  else
  {

  }
}

/*返回值：lenth of string
 * max(lenth) = 11*/
uint8_t int2str(const int32_t *number, char* string)
{
    uint8_t index = 0;
    if(*number == 0)
    {
        string[index++] = '0';
        return index;
    }
    else
    {
        uint32_t num = (*number >= 0)? *number : -*number;
        char temp;
        for(; num != 0; ++index)
        {
            string[index] = '0' + num % 10;
            num /= 10;
        }
        if(*number < 0)
        {
            string[index++] = '-';
        }
        for(uint8_t i = 0; i < index / 2; ++i)
        {
            temp = string[i];
            string[i] = string[index - i - 1];
            string[index - i - 1] = temp;
        }
        return index;
    }
}

/*返回值：转换完成后字符串索引的偏移量*/
uint8_t str2int(const char *string, int32_t *number)
{
    *number = 0;
    if(string == NULL)
    {
        return 0;
    }
    else
    {
        uint8_t index;
        /*找数字开始处*/
        for(index = 0; string[index] != '\0'; ++index)
        {
            if(string[index] >= '0' && string[index] <= '9') break;
        }
        if(string[index] != '\0')
        {
            int32_t num = 0;
            uint8_t index_start = index;
            for(; string[index] >= '0' && string[index] <= '9'; ++index)
            {
                num *= 10;
                num += string[index] - '0';
            }
            /*处理正负号*/
            if(index_start != 0)
            {
                if(string[index_start - 1] == '-') num = -num;
            }
            *number = num;
            return index;
        }
        else
        {
            return 0;
        }
    }
}

/*处理串口1*/
void refresh_uart1(void)
{
    if(uart1.string_index != -1)
    {
        return;
    }
    else
    {
        /*fix bug about '\r'*/
        char *c_r = strchr((char*)(uart1.string), '\r');
        if(c_r != NULL)
        {
            *c_r = '\n';
            *(c_r + 1) = '\0';
        }

        char *pstring = (char*)uart1.string;
        /*错误标志*/
        uint8_t error_flag = 0;
        /*指令处理*/
        if(!strncmp(pstring, "\n", 1 * sizeof(char)))
        {
            /*nothing to do*/
        }
        else if(!strncmp(pstring, "\r", 1 * sizeof(char)))
        {
            /*nothing to do*/
        }
        else if(!strncmp(pstring, "test\n", 5 * sizeof(char)))   /*test*/
        {
            char string[] = "test ack\r\n";
            HAL_UART_Transmit(&huart1, (uint8_t*)string, 10 * sizeof(char), 100);
        }
        else if(!strncmp(pstring, "move ", 5 * sizeof(char)))   /*move*/
        {
            /*move [x] [y]*/
            int32_t x = 0;
            int32_t y = 0;
            uint8_t offset = 0;
            pstring += 5;
            offset = str2int(pstring, &x);
            if(offset == 0) error_flag = 1;
            pstring += offset;
            offset = str2int(pstring, &y);
            if(offset == 0) error_flag = 1;
            if(error_flag == 0)
            {
                HAL_UART_Transmit(&huart1, (uint8_t*)"move...\r\n", 9 * sizeof(char), 100);
                move_xy_01mm(x ,y);
                HAL_UART_Transmit(&huart1, (uint8_t*)"Done\r\n", 6 * sizeof(char), 100);
            }
        }
        else if(!strncmp(pstring, "moveto ", 7 * sizeof(char)))  /*moveto*/
        {
            /*moveto [x] [y]*/
            int32_t x = 0;
            int32_t y = 0;
            uint8_t offset = 0;
            pstring += 7;
            offset = str2int(pstring, &x);
            if(offset == 0) error_flag = 1;
            pstring += offset;
            offset = str2int(pstring, &y);
            if(offset == 0) error_flag = 1;
            if(error_flag == 0)
            {
                HAL_UART_Transmit(&huart1, (uint8_t*)"move...\r\n", 9 * sizeof(char), 100);
                moveto_xy_01mm(x ,y);
                HAL_UART_Transmit(&huart1, (uint8_t*)"Done\r\n", 6 * sizeof(char), 100);
            }
        }
        else if(!strncmp(pstring, "movez ", 6 * sizeof(char)))   /*movez*/
        {
            /*movez [z]*/
            int32_t z = 0;
            pstring += 6;
            if(0 == str2int(pstring, &z)) error_flag = 1;
            if(error_flag == 0)
            {
                if (stepping_motor.z / 249 + z < MIN_Z_01MM)
                {
                    HAL_UART_Transmit(&huart1, (uint8_t*)"ERROR:z out of range\r\n", 22 * sizeof(char), 100);
                }
                else
                {
                    HAL_UART_Transmit(&huart1, (uint8_t*)"move...\r\n", 9 * sizeof(char), 100);
                    move_z_01mm(z);
                    HAL_UART_Transmit(&huart1, (uint8_t*)"Done\r\n", 6 * sizeof(char), 100);
                }
            }
        }
        else if(!strncmp(pstring, "movezto ", 8 * sizeof(char)))  /*movezto*/
        {
            /*movezto [z]*/
            int32_t z = 0;
            pstring += 8;
            if(0 == str2int(pstring, &z)) error_flag = 1;
            if(error_flag == 0)
            {
                if (z < MIN_Z_01MM)
                {
                    HAL_UART_Transmit(&huart1, (uint8_t*)"ERROR:z out of range\r\n", 22 * sizeof(char), 100);
                }
                else
                {
                    HAL_UART_Transmit(&huart1, (uint8_t*)"move...\r\n", 9 * sizeof(char), 100);
                    moveto_z_01mm(z);
                    HAL_UART_Transmit(&huart1, (uint8_t*)"Done\r\n", 6 * sizeof(char), 100);
                }
            }
        }
        else if(!strncmp(pstring, "setv ", 5 * sizeof(char)))   /*setv*/
        {
            /*setv [v]*/
            int32_t v = 0;
            pstring += 5;
            if(0 == str2int(pstring, &v)) error_flag = 1;
            if(error_flag == 0)
            {
                set_DAC(v);
                HAL_UART_Transmit(&huart1, (uint8_t*)"OK\r\n", 4 * sizeof(char), 100);
            }
        }
        else if(!strncmp(pstring, "setd ", 5 * sizeof(char)))   /*setd*/
        {
            /*setd [0/1]*/
            int32_t d = 0;
            pstring += 5;
            if(0 == str2int(pstring, &d)) error_flag = 1;
            if(d != 0 && d != 1) error_flag = 1;
            if(error_flag == 0)
            {
                set_BLDC_D(d);
                HAL_UART_Transmit(&huart1, (uint8_t*)"OK\r\n", 4 * sizeof(char), 100);
            }
        }
        else if(!strncmp(pstring, "locate\n", 7 * sizeof(char)))   /*locate*/
        {
            HAL_UART_Transmit(&huart1, (uint8_t*)"locate...\r\n", 11 * sizeof(char), 100);
            autolocate();
            HAL_UART_Transmit(&huart1, (uint8_t*)"Done\r\n", 6 * sizeof(char), 100);
        }
        else if(!strncmp(pstring, "set0\n", 5 * sizeof(char)))  /*set0*/
        {
            set0();
            HAL_UART_Transmit(&huart1, (uint8_t*)"OK\r\n", 4 * sizeof(char), 100);
        }
        else if(!strncmp(pstring, "set0xy\n", 7 * sizeof(char)))  /*set0xy*/
        {
            set0xy();
            HAL_UART_Transmit(&huart1, (uint8_t*)"OK\r\n", 4 * sizeof(char), 100);
        }
        else if(!strncmp(pstring, "set0z\n", 6 * sizeof(char)))  /*set0z*/
        {
            set0z();
            HAL_UART_Transmit(&huart1, (uint8_t*)"OK\r\n", 4 * sizeof(char), 100);
        }
        else if(!strncmp(pstring, "here\n", 5 * sizeof(char)))   /*here*/
        {
            int32_t x = stepping_motor.x / 249;
            int32_t y = stepping_motor.y / 249;
            int32_t z = stepping_motor.z / 249;
            uint8_t string[15];
            string[1] = ':';
            uint8_t size;
            /*x*/
            string[0] = 'X';
            size = 2;
            size += int2str(&x, (char*)string + size);
            string[size++] = '\r';
            string[size++] = '\n';
            HAL_UART_Transmit(&huart1, string, size * sizeof(char), 100);
            /*y*/
            string[0] = 'Y';
            size = 2;
            size += int2str(&y, (char*)string + size);
            string[size++] = '\r';
            string[size++] = '\n';
            HAL_UART_Transmit(&huart1, string, size * sizeof(char), 100);
            /*z*/
            string[0] = 'Z';
            size = 2;
            size += int2str(&z, (char*)string + size);
            string[size++] = '\r';
            string[size++] = '\n';
            HAL_UART_Transmit(&huart1, string, size * sizeof(char), 100);
        }
        else if(!strncmp(pstring, "plan\n", 5 * sizeof(char)))   /*plan*/
        {
            uint8_t string[11];
            uint8_t size;

            HAL_UART_Transmit(&huart1, (uint8_t*)"plan1:", 6 * sizeof(char), 100);
            size = int2str(&plan.x1, (char*)string);
            HAL_UART_Transmit(&huart1, string, size * sizeof(char), 100);
            HAL_UART_Transmit(&huart1, (uint8_t*)",", 1 * sizeof(char), 100);
            size = int2str(&plan.y1, (char*)string);
            HAL_UART_Transmit(&huart1, string, size * sizeof(char), 100);
            if(plan.auto1 == 0)
                HAL_UART_Transmit(&huart1, (uint8_t*)"NOTauto\r\n", 9 * sizeof(char), 100);
            else
                HAL_UART_Transmit(&huart1, (uint8_t*)"auto\r\n", 6 * sizeof(char), 100);

            HAL_UART_Transmit(&huart1, (uint8_t*)"plan2:", 6 * sizeof(char), 100);
            size = int2str(&plan.x2, (char*)string);
            HAL_UART_Transmit(&huart1, string, size * sizeof(char), 100);
            HAL_UART_Transmit(&huart1, (uint8_t*)",", 1 * sizeof(char), 100);
            size = int2str(&plan.y2, (char*)string);
            HAL_UART_Transmit(&huart1, string, size * sizeof(char), 100);
            if(plan.auto2 == 0)
                HAL_UART_Transmit(&huart1, (uint8_t*)"NOTauto\r\n", 9 * sizeof(char), 100);
            else
                HAL_UART_Transmit(&huart1, (uint8_t*)"auto\r\n", 6 * sizeof(char), 100);

            HAL_UART_Transmit(&huart1, (uint8_t*)"plan3:", 6 * sizeof(char), 100);
            size = int2str(&plan.x3, (char*)string);
            HAL_UART_Transmit(&huart1, string, size * sizeof(char), 100);
            HAL_UART_Transmit(&huart1, (uint8_t*)",", 1 * sizeof(char), 100);
            size = int2str(&plan.y3, (char*)string);
            HAL_UART_Transmit(&huart1, string, size * sizeof(char), 100);
            if(plan.auto3 == 0)
                HAL_UART_Transmit(&huart1, (uint8_t*)"NOTauto\r\n", 9 * sizeof(char), 100);
            else
                HAL_UART_Transmit(&huart1, (uint8_t*)"auto\r\n", 6 * sizeof(char), 100);

            HAL_UART_Transmit(&huart1, (uint8_t*)"plan4:", 6 * sizeof(char), 100);
            size = int2str(&plan.x4, (char*)string);
            HAL_UART_Transmit(&huart1, string, size * sizeof(char), 100);
            HAL_UART_Transmit(&huart1, (uint8_t*)",", 1 * sizeof(char), 100);
            size = int2str(&plan.y4, (char*)string);
            HAL_UART_Transmit(&huart1, string, size * sizeof(char), 100);
            HAL_UART_Transmit(&huart1, (uint8_t*)"\r\n", 2 * sizeof(char), 100);
        }
        else if(!strncmp(pstring, "plan ", 5 * sizeof(char)))   /*plan ...*/
        {
            int32_t n = 0;
            int32_t x = 0;
            int32_t y = 0;
            int32_t _auto = 0;
            uint8_t offset;
            pstring += 5;
            offset = str2int(pstring, &n);
            if(offset == 0) error_flag = 1;
            pstring += offset;
            offset = str2int(pstring, &x);
            if(offset == 0) error_flag = 1;
            pstring += offset;
            offset = str2int(pstring, &y);
            if(offset == 0) error_flag = 1;
            if(n < 4)
            {
                pstring += offset;
                offset = str2int(pstring, &_auto);
                if (offset == 0) error_flag = 1;
            }
            if(n < 1 || n > 4) error_flag = 1;
            if(error_flag == 0)
            {
                switch (n) {
                    case 1:{
                        plan.x1 = x;
                        plan.y1 = y;
                        plan.auto1 = _auto;
                        break;
                    }
                    case 2:{
                        plan.x2 = x;
                        plan.y2 = y;
                        plan.auto2 = _auto;
                        break;
                    }
                    case 3:{
                        plan.x3 = x;
                        plan.y3 = y;
                        plan.auto3 = _auto;
                        break;
                    }
                    case 4:{
                        plan.x4 = x;
                        plan.y4 = y;
                    }
                    default: break;
                }
                HAL_UART_Transmit(&huart1, (uint8_t*)"OK\r\n", 4 * sizeof(char), 100);
            }
        }
        else if(!strncmp(pstring, "fix_show\n", 9 * sizeof(char)))   /*fix_show*/
        {
            char string[11];
            uint8_t size;

            /*1*/
            HAL_UART_Transmit(&huart1, (uint8_t*)"fix1:", 5 * sizeof(char), 100);
            size = int2str(&plan.fix1_x, string);
            HAL_UART_Transmit(&huart1, (uint8_t*)string, size * sizeof(char), 100);
            HAL_UART_Transmit(&huart1, (uint8_t*)",", 1 * sizeof(char), 100);
            size = int2str(&plan.fix1_y, string);
            HAL_UART_Transmit(&huart1, (uint8_t*)string, size * sizeof(char), 100);
            HAL_UART_Transmit(&huart1, (uint8_t*)"\r\n", 2 * sizeof(char), 100);

            /*2*/
            HAL_UART_Transmit(&huart1, (uint8_t*)"fix2:", 5 * sizeof(char), 100);
            size = int2str(&plan.fix2_x, string);
            HAL_UART_Transmit(&huart1, (uint8_t*)string, size * sizeof(char), 100);
            HAL_UART_Transmit(&huart1, (uint8_t*)",", 1 * sizeof(char), 100);
            size = int2str(&plan.fix2_y, string);
            HAL_UART_Transmit(&huart1, (uint8_t*)string, size * sizeof(char), 100);
            HAL_UART_Transmit(&huart1, (uint8_t*)"\r\n", 2 * sizeof(char), 100);

            /*3*/
            HAL_UART_Transmit(&huart1, (uint8_t*)"fix3:", 5 * sizeof(char), 100);
            size = int2str(&plan.fix3_x, string);
            HAL_UART_Transmit(&huart1, (uint8_t*)string, size * sizeof(char), 100);
            HAL_UART_Transmit(&huart1, (uint8_t*)",", 1 * sizeof(char), 100);
            size = int2str(&plan.fix3_y, string);
            HAL_UART_Transmit(&huart1, (uint8_t*)string, size * sizeof(char), 100);
            HAL_UART_Transmit(&huart1, (uint8_t*)"\r\n", 2 * sizeof(char), 100);
        }
        else if(!strncmp(pstring, "fix_set ", 8 * sizeof(char)))   /*fix_set*/
        {
            /*fix_set [n] [x] [y]*/
            int32_t n = 0;
            int32_t x = 0;
            int32_t y = 0;
            uint8_t offset = 0;
            pstring += 8;
            offset = str2int(pstring, &n);
            if(0 == offset) error_flag = 1;
            pstring += offset;
            offset = str2int(pstring, &x);
            if(0 == offset) error_flag = 1;
            pstring += offset;
            offset = str2int(pstring, &y);
            if(0 == offset) error_flag = 1;
            if(n < 0 || n > 3) error_flag = 1;
            if(error_flag == 0)
            {
                switch (n) {
                    case 0:{
                        plan.fix1_x = x;
                        plan.fix1_y = y;
                        plan.fix2_x = x;
                        plan.fix2_y = y;
                        plan.fix3_x = x;
                        plan.fix3_y = y;
                        break;
                    }
                    case 1:{
                        plan.fix1_x = x;
                        plan.fix1_y = y;
                        break;
                    }
                    case 2:{
                        plan.fix2_x = x;
                        plan.fix2_y = y;
                        break;
                    }
                    case 3:{
                        plan.fix3_x = x;
                        plan.fix3_y = y;
                        break;
                    }
                    default: break;
                }
                HAL_UART_Transmit(&huart1, (uint8_t*)"OK\r\n", 4 * sizeof(char), 100);
            }
        }
        else if(!strncmp(pstring, "run\n", 4 * sizeof(char)))   /*run*/
        {
            HAL_UART_Transmit(&huart1, (uint8_t*)"run...\r\n", 8 * sizeof(char), 100);
            run();
            HAL_UART_Transmit(&huart1, (uint8_t*)"Done\r\n", 6 * sizeof(char), 100);
        }
        else if(!strncmp(pstring, "run_test\n", 9 * sizeof(char)))   /*run_test*/
        {
            HAL_UART_Transmit(&huart1, (uint8_t*)"run_test...\r\n", 13 * sizeof(char), 100);
            run_test();
            HAL_UART_Transmit(&huart1, (uint8_t*)"Done\r\n", 6 * sizeof(char), 100);
        }
        else
        {
            error_flag = 1;
        }

        if(error_flag == 1)
        {
            HAL_UART_Transmit(&huart1, (uint8_t*)"ERROR:unknown command\r\n", 23 * sizeof(char), 100);
        }
        uart1.string_index = 0;
        HAL_UART_Receive_IT(&huart1, &uart1.buffer, 1);
    }
}

void autolocate(void)
{
    uart3.x = 0xFF;
    uart3.y = 0xFF;
    uart3.string_index = 0;
    uart3.buffer = 0;
    HAL_UART_Receive_IT(&huart3, &uart3.buffer, 1 * sizeof(uint8_t));
    while(uart3.x == 0xFF || uart3.y == 0xFF);

    /*y*/
    if(uart3.y > OPENMV_FRAME_SIZE_Y / 2 + 1)
    {
        if(OPENMV_NY)
            motor_y_move(1);
        else
            motor_y_move(0);
        while (uart3.y > OPENMV_FRAME_SIZE_Y / 2 + 1);
        if(OPENMV_NY)
            motor_y_stop(1);
        else
            motor_y_stop(0);
    }
    else if(uart3.y < OPENMV_FRAME_SIZE_Y / 2 - 1)
    {
        if(OPENMV_NY)
            motor_y_move(0);
        else
            motor_y_move(1);
        while (uart3.y < OPENMV_FRAME_SIZE_Y / 2 - 1);
        if(OPENMV_NY)
            motor_y_stop(0);
        else
            motor_y_stop(1);
    }
    else{}
    delay(DELAY_DEFAULT);

    /*x*/
    if(uart3.x > OPENMV_FRAME_SIZE_X / 2 + 1)
    {
        if(OPENMV_NX)
            motor_x_move(1);
        else
            motor_x_move(0);
        while (uart3.x > OPENMV_FRAME_SIZE_X / 2 + 1);
        if(OPENMV_NX)
            motor_x_stop(1);
        else
            motor_x_stop(0);
    }
    else if(uart3.x < OPENMV_FRAME_SIZE_X / 2 - 1)
    {
        if(OPENMV_NX)
            motor_x_move(0);
        else
            motor_x_move(1);
        while (uart3.x < OPENMV_FRAME_SIZE_X / 2 - 1);
        if(OPENMV_NX)
            motor_x_stop(0);
        else
            motor_x_stop(1);
    }
    else{}
    delay(DELAY_DEFAULT);
}

void set0xy(void)
{
    stepping_motor.x = 0;
    stepping_motor.y = 0;
}

void set0z(void)
{
    stepping_motor.z = 0;
}

void set0(void)
{
    set0xy();
    set0z();
}

/*DAC output
 * v: 电压 = v * 0.1 , max = 3.3V = 33 * 0.1 */
void set_DAC(uint8_t v)
{
    if(v > 33) v = 33;
    HAL_DAC_SetValue(&hdac, DAC_CHANNEL_1, DAC_ALIGN_12B_R, v * 4095 / 33);
    HAL_DAC_Start(&hdac, DAC_CHANNEL_1);
}

void set_BLDC_D(uint8_t dir)
{
    if(MOTOR_B_ND == 0)
    {
        if(dir == 0)
            HAL_GPIO_WritePin(MOTOR_B_DIR_GPIO_Port, MOTOR_B_DIR_Pin, GPIO_PIN_RESET);
        else
            HAL_GPIO_WritePin(MOTOR_B_DIR_GPIO_Port, MOTOR_B_DIR_Pin, GPIO_PIN_SET);
    }
    else
    {
        if(dir == 0)
            HAL_GPIO_WritePin(MOTOR_B_DIR_GPIO_Port, MOTOR_B_DIR_Pin, GPIO_PIN_SET);
        else
            HAL_GPIO_WritePin(MOTOR_B_DIR_GPIO_Port, MOTOR_B_DIR_Pin, GPIO_PIN_RESET);
    }
}

void drill(void)
{
    moveto_z_01mm(Z_STAGE_0);
    set_DAC(MOTOR_D_V_HIGH);
    moveto_z_01mm(Z_STAGE_1);
    set_DAC(MOTOR_D_V_LOW);
    moveto_z_01mm(Z_STAGE_2);
    delay(DELAY_DEFAULT);
    moveto_z_01mm(Z_STAGE_0);
    set_DAC(0);
}

void run(void)
{
    if(stepping_motor.z != Z_STAGE_0)
    {
        moveto_z_01mm(Z_STAGE_0);
        delay(DELAY_DEFAULT);
    }

    /*1*/
    HAL_UART_Transmit(&huart1, (uint8_t*)"move1\r\n", 7 * sizeof(char), 100);
    moveto_xy_01mm(plan.x1, plan.y1);
    delay(DELAY_DEFAULT);
    if(plan.auto1 != 0)
    {
        HAL_UART_Transmit(&huart1, (uint8_t*)"autolocate\r\n", 12 * sizeof(char), 100);
        autolocate();
        autolocate();
        autolocate();
        delay(DELAY_DEFAULT);
    }
    else
    {
        HAL_UART_Transmit(&huart1, (uint8_t*)"NOTautolocate\r\n", 15 * sizeof(char), 100);
    }
    HAL_UART_Transmit(&huart1, (uint8_t*)"fix\r\n", 5 * sizeof(char), 100);
    move_xy_01mm(0, plan.fix1_y);
    delay(DELAY_DEFAULT);
    move_xy_01mm(plan.fix1_x, 0);
    delay(DELAY_DEFAULT);
    HAL_UART_Transmit(&huart1, (uint8_t*)"drill\r\n", 7 * sizeof(char), 100);
    drill();
    delay(DELAY_DEFAULT);

    /*2*/
    HAL_UART_Transmit(&huart1, (uint8_t*)"move2\r\n", 7 * sizeof(char), 100);
    moveto_xy_01mm(plan.x2, plan.y2);
    delay(DELAY_DEFAULT);
    if(plan.auto2 != 0)
    {
        HAL_UART_Transmit(&huart1, (uint8_t*)"autolocate\r\n", 12 * sizeof(char), 100);
        autolocate();
        autolocate();
        autolocate();
        delay(DELAY_DEFAULT);
    }
    else
    {
        HAL_UART_Transmit(&huart1, (uint8_t*)"NOTautolocate\r\n", 15 * sizeof(char), 100);
    }
    HAL_UART_Transmit(&huart1, (uint8_t*)"fix\r\n", 5 * sizeof(char), 100);
    move_xy_01mm(0, plan.fix2_y);
    delay(DELAY_DEFAULT);
    move_xy_01mm(plan.fix2_x, 0);
    delay(DELAY_DEFAULT);
    HAL_UART_Transmit(&huart1, (uint8_t*)"drill\r\n", 7 * sizeof(char), 100);
    drill();
    delay(DELAY_DEFAULT);

    /*3*/
    HAL_UART_Transmit(&huart1, (uint8_t*)"move3\r\n", 7 * sizeof(char), 100);
    moveto_xy_01mm(plan.x3, plan.y3);
    delay(DELAY_DEFAULT);
    if(plan.auto3 != 0)
    {
        HAL_UART_Transmit(&huart1, (uint8_t*)"autolocate\r\n", 12 * sizeof(char), 100);
        autolocate();
        autolocate();
        autolocate();
        delay(DELAY_DEFAULT);
    }
    else
    {
        HAL_UART_Transmit(&huart1, (uint8_t*)"NOTautolocate\r\n", 15 * sizeof(char), 100);
    }
    HAL_UART_Transmit(&huart1, (uint8_t*)"fix\r\n", 5 * sizeof(char), 100);
    move_xy_01mm(0, plan.fix3_y);
    delay(DELAY_DEFAULT);
    move_xy_01mm(plan.fix3_x, 0);
    delay(DELAY_DEFAULT);
    HAL_UART_Transmit(&huart1, (uint8_t*)"drill\r\n", 7 * sizeof(char), 100);
    drill();
    delay(DELAY_DEFAULT);

    /*return*/
    HAL_UART_Transmit(&huart1, (uint8_t*)"return\r\n", 8 * sizeof(char), 100);
    moveto_xy_01mm(plan.x4, plan.y4);
    delay(DELAY_DEFAULT);
}

/* run without drilling*/
void run_test(void)
{
    if(stepping_motor.z != Z_STAGE_0)
    {
        moveto_z_01mm(Z_STAGE_0);
        delay(DELAY_DEFAULT);
    }

    /*1*/
    HAL_UART_Transmit(&huart1, (uint8_t*)"move1\r\n", 7 * sizeof(char), 100);
    moveto_xy_01mm(plan.x1, plan.y1);
    delay(DELAY_DEFAULT);
    if(plan.auto1 != 0)
    {
        HAL_UART_Transmit(&huart1, (uint8_t*)"autolocate\r\n", 12 * sizeof(char), 100);
        autolocate();
        autolocate();
        autolocate();
        delay(DELAY_DEFAULT);
    }
    else
    {
        HAL_UART_Transmit(&huart1, (uint8_t*)"NOTautolocate\r\n", 15 * sizeof(char), 100);
    }
    HAL_UART_Transmit(&huart1, (uint8_t*)"fix\r\n", 5 * sizeof(char), 100);
    move_xy_01mm(0, plan.fix1_y);
    delay(DELAY_DEFAULT);
    move_xy_01mm(plan.fix1_x, 0);
    delay(DELAY_DEFAULT);
    moveto_z_01mm(10);
    delay(6 * DELAY_DEFAULT);
    moveto_z_01mm(Z_STAGE_0);
    delay(DELAY_DEFAULT);

    /*2*/
    HAL_UART_Transmit(&huart1, (uint8_t*)"move2\r\n", 7 * sizeof(char), 100);
    moveto_xy_01mm(plan.x2, plan.y2);
    delay(DELAY_DEFAULT);
    if(plan.auto2 != 0)
    {
        HAL_UART_Transmit(&huart1, (uint8_t*)"autolocate\r\n", 12 * sizeof(char), 100);
        autolocate();
        autolocate();
        autolocate();
        delay(DELAY_DEFAULT);
    }
    else
    {
        HAL_UART_Transmit(&huart1, (uint8_t*)"NOTautolocate\r\n", 15 * sizeof(char), 100);
    }
    HAL_UART_Transmit(&huart1, (uint8_t*)"fix\r\n", 5 * sizeof(char), 100);
    move_xy_01mm(0, plan.fix2_y);
    delay(DELAY_DEFAULT);
    move_xy_01mm(plan.fix2_x, 0);
    delay(DELAY_DEFAULT);
    moveto_z_01mm(10);
    delay(6 * DELAY_DEFAULT);
    moveto_z_01mm(Z_STAGE_0);
    delay(DELAY_DEFAULT);

    /*3*/
    HAL_UART_Transmit(&huart1, (uint8_t*)"move3\r\n", 7 * sizeof(char), 100);
    moveto_xy_01mm(plan.x3, plan.y3);
    delay(DELAY_DEFAULT);
    if(plan.auto3 != 0)
    {
        HAL_UART_Transmit(&huart1, (uint8_t*)"autolocate\r\n", 12 * sizeof(char), 100);
        autolocate();
        autolocate();
        autolocate();
        delay(DELAY_DEFAULT);
    }
    else
    {
        HAL_UART_Transmit(&huart1, (uint8_t*)"NOTautolocate\r\n", 15 * sizeof(char), 100);
    }
    HAL_UART_Transmit(&huart1, (uint8_t*)"fix\r\n", 5 * sizeof(char), 100);
    move_xy_01mm(0, plan.fix3_y);
    delay(DELAY_DEFAULT);
    move_xy_01mm(plan.fix3_x, 0);
    delay(DELAY_DEFAULT);
    moveto_z_01mm(10);
    delay(6 * DELAY_DEFAULT);
    moveto_z_01mm(Z_STAGE_0);
    delay(DELAY_DEFAULT);
}
/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */

  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
